import time, network
from Maix import GPIO
from machine import UART
from fpioa_manager import fm
from board import board_info
WIFI_SSID   = "wyc"
WIFI_PASSWD = "12345678"
#WIFI_SSID   = "project_my"
#WIFI_PASSWD = "12345678"
class wifi():

    __is_m1w__ = True
    uart = None
    eb = None
    nic = None

    def init():
        if __class__.__is_m1w__:
            fm.register(0, fm.fpioa.GPIOHS1, force=True)
            M1wPower=GPIO(GPIO.GPIOHS1, GPIO.OUT)
            M1wPower.value(0) # b'\r\n ets Jan  8 2013,rst cause:1, boot mode:(7,6)\r\n\r\nwaiting for host\r\n'

        fm.register(8, fm.fpioa.GPIOHS0) # board_info.WIFI_EN == IO 8
        __class__.en = GPIO(GPIO.GPIOHS0,GPIO.OUT)

        fm.register(7,fm.fpioa.UART2_TX) # board_info.WIFI_RX == IO 7
        fm.register(6,fm.fpioa.UART2_RX) # board_info.WIFI_TX == IO 6
        __class__.uart = UART(UART.UART2, 115200, timeout=1000, read_buf_len=8192)

    def enable(en):
        __class__.en.value(en)

    def _at_cmd(cmd="AT\r\n", resp="OK\r\n", timeout=20):
        __class__.uart.write(cmd) # "AT+GMR\r\n"
        time.sleep_ms(timeout)
        tmp = __class__.uart.read()
        # print(tmp)
        if tmp and tmp.endswith(resp):
            return True
        return False

    def at_cmd(cmd="AT\r\n", timeout=20):
        __class__.uart.write(cmd) # "AT+GMR\r\n"
        time.sleep_ms(timeout)
        tmp = __class__.uart.read()
        return tmp

    def reset(force=False, reply=5):
        if force == False and __class__.isconnected():
            return True
        __class__.init()
        for i in range(reply):
            print('reset...')
            __class__.enable(False)
            time.sleep_ms(50)
            __class__.enable(True)
            time.sleep_ms(500) # at start > 500ms
            if __class__._at_cmd(timeout=500):
                break
        __class__._at_cmd()
        __class__._at_cmd('AT+UART_CUR=921600,8,1,0,0\r\n', "OK\r\n")
        __class__.uart = UART(UART.UART2, 921600, timeout=1000, read_buf_len=10240)
        # important! baudrate too low or read_buf_len too small will loose data
        #print(__class__._at_cmd())
        try:
            __class__.nic = network.ESP8285(__class__.uart)
            time.sleep_ms(500) # wait at ready to connect
        except Exception as e:
            print(e)
            return False
        return True

    def connect(ssid="wifi_name", pasw="pass_word"):
        if __class__.nic != None:
            return __class__.nic.connect(ssid, pasw)

    def ifconfig(): # should check ip != 0.0.0.0
        if __class__.nic != None:
            return __class__.nic.ifconfig()

    def isconnected():
        if __class__.nic != None:
            return __class__.nic.isconnected()
        return False
    def check_wifi_net(reply=5):
        if wifi.isconnected() != True:
            for i in range(reply):
                try:
                    wifi.reset()
                    print('try AT connect wifi...', wifi._at_cmd())
                    wifi.connect(WIFI_SSID, WIFI_PASSWD)
                    if wifi.isconnected():
                        break
                except Exception as e:
                    print(e)
        return wifi.isconnected()
wifi.check_wifi_net()

#addr       = ("192.168.43.6", 8000)
addr       = ("192.168.43.101", 8000)
##################################

import socket, time, sensor, image
import lcd
import KPU as kpu
from fpioa_manager import fm
from machine import UART
from Maix import FPIOA,GPIO

clock = time.clock()
lcd.init()
lcd.rotation(2)
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)

#task_fd = kpu.load(0x300000) # 从flash 0x200000 加载人脸检测模型
#task_ld = kpu.load(0x400000) # 从flash 0x300000 加载人脸五点关键点检测模型
#task_fe = kpu.load(0x500000) # 从flash 0x400000 加载人脸196维特征值模型

task_fd = kpu.load("/sd/model/FD_face.smodel") # 加载人脸检测模型
task_ld = kpu.load("/sd/model/KP_face.smodel") # 加载人脸五点关键点检测模型
task_fe = kpu.load("/sd/model/FE_face.smodel") # 加载人脸196维特征值模型

#fm.register(5, fm.fpioa.UART1_TX, force=True)#fm.fpioa.UART1_TX是K210上的TX
#fm.register(4, fm.fpioa.UART1_RX, force=True)

fm.register(9, fm.fpioa.UART1_TX, force=True)#fm.fpioa.UART1_TX是K210上的TX
fm.register(10, fm.fpioa.UART1_RX, for =ce=True)#中间

uart_A = UART(UART.UART1, 115200, 8, 0, 1, timeout=1000, read_buf_len=4096)
#uart_B = UART(UART.UART2, 115200, 8, 1, 0, timeout=1000, read_buf_len=4096)

clock = time.clock()  # 初始化系统时钟，计算帧率
key_pin=16 # 设置按键引脚 FPIO16
fpioa = FPIOA()
fpioa.set_function(key_pin,FPIOA.GPIO7)
key_gpio=GPIO(GPIO.GPIO7,GPIO.IN)
last_key_state=1
key_pressed=0 # 初始化按键引脚 分配GPIO7 到 FPIO16
def check_key(): # 按键检测函数，用于在循环中检测按键是否按下，下降沿有效
    global last_key_state
    global key_pressed
    val=key_gpio.value()
    if last_key_state == 1 and val == 0:
        key_pressed=1
    else:
        key_pressed=0
    last_key_state = val

anchor = (1.889, 2.5245, 2.9465, 3.94056, 3.99987, 5.3658, 5.155437, 6.92275, 6.718375, 9.01025) #anchor for face detect 用于人脸检测的Anchor
dst_point = [(44,59),(84,59),(64,82),(47,105),(81,105)] #standard face key point position 标准正脸的5关键点坐标 分别为 左眼 右眼 鼻子 左嘴角 右嘴角
a = kpu.init_yolo2(task_fd, 0.5, 0.3, 5, anchor) #初始化人脸检测模型
img_lcd=image.Image() # 设置显示buf
img_face=image.Image(size=(128,128)) #设置 128 * 128 人脸图片buf
a=img_face.pix_to_ai() # 将图片转为kpu接受的格式
record_ftr=[] #空列表 用于存储当前196维特征
record_ftrs=[bytearray(b'\xda\xee\x02P\xac\x19i\x19\x06%\x16\r\xcd\xfb\xda\xb5\xf6\xaa\xc1\x0b\xd1\x1d\xcd\xe7\xd3\x02\xea\x9d\x1b\x0e\xfe.\xad\x1a\r\x13FS\xdd\x03\xda\xdd/\xe3\xff\x05\xdd?\xf5\x0b\xdd\xf7\x91\xdb0\x064\x06\xe1\x1a\xc2\xe5\x16\xf6\x13e:|>\xd2/\xba\xf6N\x05\xbd\xca>\xf1\xeb\x0e\x1a\x9f/\x00\xed*\xce\xee\x1f\xc0O%*FK\xa4\xd7\xf7\xd5\xea\xf2\t\x19h\xca\xfb\x01\x12\x11\n\xee\xd7\x17\r\xd1\xf9\xdd\xc0;\xd0\xe7\x93\xdf+\xf3*\xc5\xef\xdf\xfe\xe9!\x01)i_\xd2\xc2\xd0\xce\xcc\x11\xdf\x1f\\)\xac\x80\xa6\xef\xd9\x0e\xe5\xef")\xedF\xf6\r\'H#\xfa\xde\x013c\xfdB\xb0\x0et\x1b\x06\xbc\\\xee\xfa;V\n0\x0e;\xfd\xf7*\x03\xc1\x99\xb5\xea\xdd\xdf')] #空列表 用于存储按键记录下人脸特征， 可以将特征以txt等文件形式保存到sd卡后，读取到此列表，即可实现人脸断电存储。
names = ['Mr.wyc', 'Mr.lw', 'Mr.lyq', 'Mr.4', 'Mr.5', 'Mr.6', 'Mr.7', 'Mr.8', 'Mr.9' , 'Mr.10'] # 人名标签，与上面列表特征值一一对应。

aa=0#判断是否识别成功，来决定是否向pc发送pic


while True:
    # send pic
    while True:
        try:
            sock = socket.socket()
            print(sock)
            sock.connect(addr)
            break
        except Exception as e:
            print("connect error:", e)
            #sock.close()
            #continue
    sock.settimeout(5)

    send_len, count, err = 0, 0, 0
    while True:
        check_key() #按键检测
        #uart_A.write('a')
        img = sensor.snapshot() #从摄像头获取一张图片
        clock.tick() #记录时刻，用于计算帧率
        code = kpu.run_yolo2(task_fd, img) # 运行人脸检测模型，获取人脸坐标位置
        if code: # 如果检测到人脸
            for i in code: # 迭代坐标框
                # Cut face and resize to 128x128
                a = img.draw_rectangle(i.rect()) # 在屏幕显示人脸方框
                face_cut=img.cut(i.x(),i.y(),i.w(),i.h()) # 裁剪人脸部分图片到 face_cut
                face_cut_128=face_cut.resize(128,128) # 将裁出的人脸图片 缩放到128 * 128像素
                a=face_cut_128.pix_to_ai() # 将猜出图片转换为kpu接受的格式
                #a = img.draw_image(face_cut_128, (0,0))
                # Landmark for face 5 points
                fmap = kpu.forward(task_ld, face_cut_128) # 运行人脸5点关键点检测模型
                plist=fmap[:] # 获取关键点预测结果
                le=(i.x()+int(plist[0]*i.w() - 10), i.y()+int(plist[1]*i.h())) # 计算左眼位置， 这里在w方向-10 用来补偿模型转换带来的精度损失
                re=(i.x()+int(plist[2]*i.w()), i.y()+int(plist[3]*i.h())) # 计算右眼位置
                nose=(i.x()+int(plist[4]*i.w()), i.y()+int(plist[5]*i.h())) #计算鼻子位置
                lm=(i.x()+int(plist[6]*i.w()), i.y()+int(plist[7]*i.h())) #计算左嘴角位置
                rm=(i.x()+int(plist[8]*i.w()), i.y()+int(plist[9]*i.h())) #右嘴角位置
                a = img.draw_circle(le[0], le[1], 4)
                a = img.draw_circle(re[0], re[1], 4)
                a = img.draw_circle(nose[0], nose[1], 4)
                a = img.draw_circle(lm[0], lm[1], 4)
                a = img.draw_circle(rm[0], rm[1], 4) # 在相应位置处画小圆圈
                # align face to standard position
                src_point = [le, re, nose, lm, rm] # 图片中 5 坐标的位置
                T=image.get_affine_transform(src_point, dst_point) # 根据获得的5点坐标与标准正脸坐标获取仿射变换矩阵
                a=image.warp_affine_ai(img, img_face, T) #对原始图片人脸图片进行仿射变换，变换为正脸图像
                a=img_face.ai_to_pix() # 将正脸图像转为kpu格式
                #a = img.draw_image(img_face, (128,0))
                del(face_cut_128) # 释放裁剪人脸部分图片
                # calculate face feature vector
                fmap = kpu.forward(task_fe, img_face) # 计算正脸图片的196维特征值
                feature=kpu.face_encode(fmap[:]) #获取计算结果
                reg_flag = False
                scores = [] # 存储特征比对分数
                for j in range(len(record_ftrs)): #迭代已存特征值
                    score = kpu.face_compare(record_ftrs[j], feature) #计算当前人脸特征值与已存特征值的分数
                    scores.append(score) #添加分数总表
                max_score = 0
                index = 0
                for k in range(len(scores)): #迭代所有比对分数，找到最大分数和索引值
                    if max_score < scores[k]:
                        max_score = scores[k]
                        index = k
                if max_score > 85: # 如果最大分数大于85， 可以被认定为同一个人
                    a = img.draw_string(i.x(),i.y(), ("%s :%2.1f" % (names[index], max_score)), color=(0,255,0),scale=2) # 显示人名 与 分数
                    #uart_A.write("1")#给32说明人脸识别成功
                else:
                    a = img.draw_string(i.x(),i.y(), ("X :%2.1f" % (max_score)), color=(255,0,0),scale=2) #显示未知 与 分数
                if key_pressed == 1: #如果检测到按键
                    if max_score > 85:
                        uart_A.write("1")
                        #print("1")
                    else:
                        uart_A.write(" 1")
                        aa=1;
                        #print("2")
                #if key_pressed == 1: #如果检测到按键
                    #key_pressed = 0 #重置按键状态
                    #record_ftr = feature
                    #record_ftrs.append(record_ftr) #将当前特征添加到已知特征列表
                break
        fps =clock.fps() #计算帧率
        #print("%2.1f fps"%fps) #打印帧率
        a = lcd.display(img) #刷屏显示
        if aa == 1:#串口接收到1截取当前照片发送给PC
            clock.tick()
            if err >=10:
                print("socket broken")
                break
            #img = sensor.snapshot()
            #lcd.display(img)
            #img = img.to_grayscale(copy=False)
            img = img.compress(quality=30)
            img_bytes = img.to_bytes()
            print("send len: ", len(img_bytes))
            try:
                block = int(len(img_bytes)/2048)
                for i in range(block):
                    send_len = sock.send(img_bytes[i*2048:(i+1)*2048])
                    #time.sleep_ms(500)
                send_len2 = sock.send(img_bytes[block*2048:])
                #send_len = sock.send(img_bytes[0:2048])
                #send_len = sock.send(img_bytes[2048:])
                #time.sleep_ms(500)
                if send_len == 0:
                    raise Exception("send fail")
            except OSError as e:
                if e.args[0] == 128:
                    print("connection closed")
                    break
            except Exception as e:
                print("send fail:", e)
                time.sleep(1)
                err += 1
                continue
            count += 1
            print("send:", count)
            print("fps:", clock.fps())
            aa=0
            time.sleep_ms(500)
    print("close now")
    sock.close()

